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There are two goals for autonomous vehicle navigation planning: shortest 
path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearance between the vehicle and obstacles. Because a Voronoi boundary is the 
set of points locally maximizing the clearance from obstacles-, safety is maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 
autonomous vehicles. 

We use the derivative of curvature k of the vehicle motion ( dK/ds ) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
11 at the Naval Postgraduate School used a path tracking method. Before the 
mission began the vehicle 'was given a track to follow; motion planning consisted 
of calculating the point on the track closest to the vehicle and calculating dK/ds 
then steering the vehicle to get onto the track. 

We propose a method of planning safe motions of the vehicle to calculate 
optimal dK/ds at each point directly from the information of the world without 
calculating the track to follow. This safe navigation algorithm is fundamentally 
different from path tracking using a path specification. Additionally, motion 
planning is simpler and faster than the path tracking method. 

The effectiveness of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the autonomous mobile 
robot Yamabico 11 at the Naval Postgraduate School. 
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THESIS DISCLAIMER 



The reader is cautioned that computer programs developed in this research 
may not have been exercised for all cases of interest. While every effort has been 
made, within the time available, to ensure that the programs are free of 
computational and logic errors, they cannot be considered validated. Any 
application of these programs without additional verification is at the risk of the 
user. 
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EXECUTIVE SUMMARY 



There are two goals for autonomous vehicle navigation planning: shortest 
path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearances between the vehicle and obstacles. Because a Voronoi boundary is the 
set of points locally maximizing the clearance from obstacles, safety is maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 
autonomous vehicles. 

We use the derivative of curvature k of the vehicle motion (dK/ds) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
11 at the Naval Postgraduate School used a path tracking method. Before the 
mission began the vehicle was given a track to follow; motion planning consisted 
of calculating the point on the track closest to the vehicle and calculating dK/ds 
then steering the vehicle to get onto the track. The safest path through the world 
navigated by the vehicle is the set of points locally maximizing the clearance .from 
obstacles. This path is represented by the Voronoi diagram. To achieve the path 
tracking method it is necessary to calculate the Voronoi boundary which consists 
of line segments and parabolic arcs. If the world is large, it is complicated and 
inefficient to calculate every Voronoi boundary of this world. It is better to 
calculate optimal dK/ds at each point directly from the information of the world 
without calculating the track to follow. 

When the objects are two points, two lines, or one point and one line, we can 
safely navigate the vehicle to achieve equal clearances from these objects. The 
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motion of the vehicle is optimized at each point directly from the information of 
the obstacles near the vehicle. After calculating dK/ds, the vehicle follows the 
Voronoi boundaries defined by the environment. 

Unlike the path tracking method, this method can be applied to avoid moving 
objects since we calculate the optimal motion of the vehicle at each point directly 
from the information of the world. Additionally, motion control is simpler and 
faster than in the path tracking method. 

The effectiveness of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the autonomous mobile 
robot Yamabico 11 at the Naval Postgraduate School. It has precise knowledge of 
its location in a given environment using its sonar system. The robot is 
programmed by the high level mobile robot language called MML (Model-based 
Mobile robot Language) written in the C language. 
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I. INTRODUCTION 



A. BACKGROUND 

There are two goals for planning autonomous vehicle navigation planning: 
shortest path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearance between the vehicle and obstacles. Because a Voronoi boundary is the 
set of points locally maximizing the clearance from obstacles, safety is maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 
autonomous vehicles. 

B. OVERVIEW 

We use the derivative of curvature k of the vehicle motion (die/ as) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
1 1 at the Naval Postgraduate School used a path tracking method [Ref. 2]. Before 
the mission began the vehicle was given a track to follow; motion planning 
consisted of calculating the point on the track closest to the vehicle and 
calculating dK/ds then steering the vehicle to get onto the track. The safest path 
through the world navigated by the vehicle is the set of points locally maximizing 
the clearance from obstacles. This path is represented by the Voronoi diagram. To 
achieve the path tracking method it is necessary to calculate the Voronoi 
boundary which consists of line segments and parabolic arcs. If the world is large, 
it is complicated and inefficient to calculate every Voronoi boundary of this 
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world. It is better to calculate optimal dK/ds at each point directly from the 
information of the world without calculating the track to follow. 

This safe navigation algorithm is fundamentally different from path tracking 
using a path specification. Additionally, motion planning is simpler and faster than 
in the path tracking method. 

The effectiveness of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the autonomous mobile 
robot Yamabico 11 at the Naval Postgraduate School. It has precise knowledge of 
its location in a given environment using its sonar system. The robot is 
programmed by the high level mobile robot language called MML (Model-based 
Mobile robot Language) written in the C language [Ref. 3]. 
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II. PROBLEM STATEMENT 



The problems addressed are as follows: 

1 . How to navigate the robot safely to achieve the same clearance from two 
points. 

2. How to navigate the robot safely to achieve the same clearance from two 
lines. 

3. How to navigate the robot safely to achieve the same clean e from one 
point and one line. 

4. How to execute the safest path by a real robot. 

Safety is one of the important attributes for autonomous vehicle navigation 
planning. Safety is determined by the clearance between the vehicle and objects. 
Assume the vehicle is a point object and W(p) is the clearance of the point p. The 
clearance of a path represents its safety. If the clearance is small, the path is 
dangerous because it is close to the object and if it is larger, the path is safer. The 

clearance of a path n is defined as: 

W(U) = MinW(P) (2.1) 

Pen 

Where n is the path of the vehicle from start S to goal G. 

We want to find the path n o such that Vf(n o ) is the maximum among all 

possible paths (Figure 2.1). To maximize the clearance If (FI), we will take the 
Voronoi boundary as the path of the vehicle. 



3 




Figure 2.1 : Safety Path 



4 



III. VORONOI DIA .RAM 



A. DEFINITIONS 

Assume that there is an object o in a plane. It might be a point, a line, a line 
segment, a polygon, or other closed sets of points. If a world W has more than one 
object, a Voronoi region of an object o, in the world is defined as 

V(o i ) = [P\Vj[i *j -4 {dist{p,Oi)< dist(p,Oj )}]] (3.1) 

Where p is a point which is not inside o, and dist(p,o i ) is the minimum 
distance between p and o, . 

The set of all the Voronoi regions is called the Voronoi diagram of a world. 
The boundaries of Voronoi regions are Voronoi boundaries. The Voronoi 
diagram of a geometric world typically consists of lines, rays, line segments, and 
parabolic arcs. 

B. VORONOI DIAGRAM OF TWO POINTS 

In the case that a world consists of two distinct points, p x and p 2 , its Voronoi 
boundary is the bisector of those two points which generate two Voronoi regions 
V( Pl ) and V(p 2 ) (Figure 3.1). 





A=(*i.yi) 


Voronoi boundary 

1 


• 


V( A) 


i 


P 




1 V(p 2 ) 




\={x 2 ,y 2 ) 



Figure 3.1 : Voronoi Diagram of Two Points 
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C. VORONOI DIAGRAM OF TWO LINES 

In the case that a world consists of two lines 4 and which are not parallel 

to each other, their Voronoi boundaries consist of the bisectors of two lines which 
generate eight Voronoi regions V(L„), V(I, 2 ), V(L, 3 ), V (L^), V(L„), V^), 
V(l 2 3 ) and VXLm) (Figure 3.2). 




Figure 3.2 : Voronoi Diagram of Two Lines (Not Parallel) 

D. VORONOI DIAGRAM OF TWO PARALLEL LINES 

In the case that a world consists of two parallel lines L, and L^, their Voronoi 
boundary is one line which is parallel to and which generates four Voronoi 
regions V(L,j), V^), ViL^) and 7(1^), (Figure 3.3). 
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V ( L n) 

A 

Voronoi boundary v/(lj 2 ) 



v<M 

A 

w*) 



Figure 3.3 : Voronoi Diagram of Two Parallel Lines 

E. VORONOI DIAGRAM OF A LINE SEGMENT 

In the case that a world consists of one closed line segment p { p 2 , we treat it 
as a union of three objects : two end points p v p 2 and an open line segment 
e = ~pjT 2 . A closed line segment includes both endpoints, but an open line segment 
does not. Therefore, its Voronoi boundaries consists of two lines which generate 
four Voronoi regions V(p,), V(p 2 ), VC^) and V(e 2 ). (Figure 3.4) 






V(e,l 


^ — 


— 



e = p 1 p 2 
V(e 2 ) 



Voronoi boundaries 



P2 V (P2 ) 



Figure 3.4 Voronoi Diagram of A Line Segment 
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F. VORONOI DIAGRAM OF A POINT AND A LINE 



In the case that a world consists of a point p f and a line q 0 , its Voronoi 
boundary is a parabola which generates three Voronoi regions V(p f ), V(e ,), and 
V(e 2 ). The point p f is the focus and the line q 0 is the directrix of the parabola 

(Figure 3.5). 




G. VORONOI DIAGRAM OF A POINT AND TWO LINES 

In the case that a world consists of a point p that is between two parallel 
lines Ij and I „ their Voronoi boundaries are two parabolas (focus p, directrix Z, 
and focus p, directrix Lf) and the bisector of the line I* and Lj which generate 
five Voronoi regions V(p), V(ey), V(e 2 ), V(e 2 ) and V(e 4 ) (Figures 3.6 and 3.7). 
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w 



V(e 4 ) 



V(e 3 ) 




Fig 3.6 : Voronoi Diagram of A Center Point and Two Lines 



V(e 4 ) 



parabola 




Figure 3.7 : Voronoi Diagram of A Point and Two Lines 



H. VORONOI DIAGRAM OF LINE AND A RAY 

Assume a ray is a kind of line which has only one end point p. In the case 
that a world consists of a line I, and a ray L which is orthogonal to the line L,, 
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their Voronoi boundaries are a line segment p,p 2 , and two bisectors of the line L, 
and the ray L 7 , and a parabola (focus p and directrix Lj), which generates five 
Voronoi regions V(p), V(e,), V(e 2 ), V(e 3 ) and V(e 4 ) (Figure 3.8). 




L VORONOI DIAGRAM OF TWO LINES AND A LINE SEGMENT 

In the case that a world consists of two parallel lines Lj, Lj and a closed line 
segment p,p 2 , which is parallel to lines 1^ and L,, their Voronoi boundaries are 
bisectors of the line Z, and L 7 , the bisector of the line and the line segment 
p x p 2 , the bisector of the line and the line segment p,p 2 , two closed line 
segment p 3 p 4 and p 5 p 6 and parabolas (Figures 3.9 and 3.10). 
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Figure 3.9 : Voronoi Diagram of Two Lines and A Center Line Segment 




Figure 3.10 : Voronoi Diagram of Two Lines and A Line Segment 
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J. VORONOI DIAGRAM OF A NORMAL POLYGON 



In the case that a world has only one normal polygon, we treat it as a union 
of vertices (points) p , and open edges e r There are Voronoi boundaries to the 

polygon shown in (Figure 3.11). 




K. VORONOI DIAGRAM OF AN INVERTED POLYGON 

In the case that a world has only one inverted polygon, we also treat it as a 
union of vertices (points) p, and open edges e { . There are Voronoi boundaries in 

its interior (Figure 3.12). 
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IV. CURVE GENERATION 



A. CONFIGURATION 

To navigate a rigid body robot vehicle, the vehicle's state can be described by 
its current configuration, 

q = (p,6) (4.1) 

where p is the vehicle's current coordinate position (x,y), and 6 is the vehicle's 
tangent orientation at that point. 

Let k be the derivative of tangent orientation with respect to the length 
along the trajectory s which is called curvature. 



K(S) = 



ms) 

ds 



(4.2) 



In this case, k is not included in the configuration. However, k can be included 
in the configuration if k is required for the calculation. 



B. NEXT FUNCTION 

In order to compute the sequence of configurations, it is suffice to compute 
the next configuration at each step As. Given As and we can estimate the 

vehicle's next configuration at As as follows. 

1. Short Circular Segment 

The short path segment between s and 5+ As is approximated by a circular 
curve segment (Figure 4.1). 
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Assume configurations of both end points are q 0 and q v 
4o=((*o>:vo)>0o)=((o>O)>O) 



q i = ((xi,y,),0i) 

Let us assume Ad * 0. The radius of the short circular segment is 

_ As_ 
r ~ AO 



(4.3) 

(4.4) 



(4.5) 



Where 



AO = 0, - 6 0 



(4.6) 
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The configuration of the end point q x is 



<h = 




f f rsin(A0) N>v 

/(l-cos(A0)), 

A6 



(r 

V 



(sin(A0)/A0)As V 

((1-cos(A0))/A0)Aj J 
Ad 



(if Ad * 0) 



(f v Y\ 



?i = 



u. 

0! 



f(oW 

. 0 . 



0 , 



(if Ad = 0) 

Using the Taylor expansion forms for the sin and cos functions 



si n(Afl) _ Ad - (Afl) 3 /3!+ (Afl) s /5!+ _ Y (A fl) 2 ^ ( Ad ) 4 
0 A0 3! 5! 

.os(A6>) _ 1 - (1 - (Afl) 2 /2!+ (Ad) 4 / 4!- (Afl) 6 /6!- ••) 
A0 A0 

1,2! 4! 6! J 

Therefore, the configuration of the point q x is 



<h = 



(fy SA 



uJ 



r (1 - (A0) 2 /3! + (A0) 4 /5! )As 

,(1/2! - (A0) 2 /4! + (A6>) 4 /6! — • -)AdAs 

Ad 



\\ 

J 

J 



In general. 



Ad « 1 



Therefore equation (4.11) can be approximated as 



?i = 




(f 

V 

v 



(l-(A0)73!)Ar 

(1/2!-(A0) 2 /4!)A0Aj 

Ad 



\\ 

J 

J 



(4.7) 



(4.8) 



(4.9) 



(4.10) 



(4.11) 



(4.12) 



(4.13) 
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2. Global Position Calculation 

By using the composition function o [Ref. 1] which is a two dimensional 
coordinate transformation from the local coordinate system Cx ] ,y 1 ) to the global 
coordinate system Cx 0 ,y 0 ), we can calculate the global position of the vehicle at 

s+As as follows. 



q(s + As) = q 0 o q x = 



r x 0 + x, cos 6 0 - y, sin 6 0 ^ 
y 0 + x, sin 6 0 + y, cos 6 0 
< e \ +6 2 

Where q 0 and q x are given Equation (4.3) and Equation (4.13). 



(4.14) 
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V . SAFE NAVIGATION USING A STEERING FUNCTION 



We use the derivative of curvature as the only control variable for the 
vehicle. 



Therefore the vehicle's motion is controlled only through changing its curvature. 



where a,b,c are positive constants and A k, A 6 , Ad are variables. Each 
evaluation of A k. Ad, Ad differs in the situation of the obstacles (in the case that 
the obstacles are one point and one line, the obstacles are two lines, and the 
obstacles are two points). 

A. LINE TRACKING 

Consider a special case of the line tracking (Figure 5.1). Assume we want to 
track the X-axis of the global coordinate system, then we can define three 
positive constants a,b,c . 



— = f(p,e, k) 
ds 



(5.1) 




(5.2) 



We propose the steering function in the following form 




ds 



(5.3) 



or 



h oAk + bA6 + cAd = 0 

ds 



(5.4) 
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On the X-axis, 



*<■» = 0 im = o 



Therefore 

AK=K-K im = K 

A6 = e-e im = e 

lm 

Ad = y 

Let us now consider a short path segment (Figure 5.2). 



(5.5) 

(5.6) 

(5.7) 

(5.8) 




Figure 5.2 : Short Path Segment 
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The vehicle's tangent orientation 6 is specified by 



Then 



Ay is defined as 

Therefore 

Ay _ ->/(Ax) 2 + (Ay) 2 
Ax Ax 

From the definition 



,an0 = ^ 
Ax 


(5.9) 


0 = tan -1 — = tan -1 y' (as Ax — > 0) 
Ax 

, <y> 5 oo 5 

^ 3 5 


(5.10) 


As = yj (Ax) 2 + (Ay) 2 


(5.11) 



= *J\ + (y' ) 2 (as Ax -» 0) (5.12) 




-y 

—(tan -1 y ) = — ^ — — 
dx 1 + (y’ ) 2 



.« \2 



i+(y> 



(5.13) 



From Equation (5.12) and Equation (5.13) 



K 



de 

ds 



= -j-Ctan" 1 y' ) = 

ds 



-f(tan-'y) 

dx 

ds 

dx 



r 

i + (y ) 2 _ r 

Tl + OO 7 (l + (y') 2 )^ 



Therefore 



( r 14) 



UK 3 

dK_~^_ a +(y) 2 ) 2 
<*y ^ V i+ (y) 2 

dx 



y ,, (i+(y) 2 r 2 -3y(y’) 2 (i+(y) 2 r 3 (5.15) 
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Assume 





y 2 « 1 


(5.16) 




y(y”) 2 « y M 


(5.17) 


Then from Equation (5.10) 








> 

II 


(5.18) 


From Equation (5.14) 








II 

< 


(5.19) 


From Equation (5.15) 


dK _ ... 

ds y 






(5.20) 


Equation (5.4) becomes an 


ordinary differential equation: 






y " +ay" +by' +cy = 0 


(5.21) 




(D 3 + aD 2 +bD + c)y = 0 


(5.22) 



Since this is a third order linear homogeneous ordinary differential equation with 
constant coefficients, it must have at least one real root. If it has a non-negative 
root, it does not have a converging solution. If it has a complex conjugate root, 
the solution oscillates even if it decays. Since we want non-oscillatory decaying 
solutions. Equation (5.22) must have three negative roots of D. Also if we want a 
critical damping solution then Equation (5.22) must be specified to have a triple 
root, -k (where k > 0). 

D 3 + aD 2 + bD + c = (D + k) 2 =D 3 + 3kD 2 + 3k 2 D + k 3 (5.23) 

Therefore if we choose 

a = 3k (5.24) 

b = 3k 2 (5.25) 

c = k 3 (5.26) 



Equation (5.22) becomes 
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(D + k?y = 0 



(5.27) 



Thus, under this condition, there is only one degree of freedom in choosing the 
parameter k instead of three parameters a, b and c. We define 



This size constant 5 0 controls the distance for which the vehicle runs before it 

gets on track. A smaller size constant makes the transition distance smaller. Thus 
s 0 controls the sharpness of the trajectory. From Equations (5.3), (5.24), (5.25), 

(5.26) and (5.28), the revised steering function becomes 



A k. Ad, Ad are evaluated depending upon the environment. Let consider 
three situations for vehicle navigation: the objects are two points, the objects are 
two lines, the objects are one point and one line. 

B. TWO POINTS 

When we navigate the vehicle safely to make the same clearance from two 
points, its trajectory becomes a line which is a bisector of the two points. It is a 
Voronoi boundary of the two points. Let consider the case of the world consists 
of two points p l and p 2 (Figure 5.3). 




(5.28) 




(5.29) 
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1. Evaluation of Ak 

When the vehicle's configuration is q = (p,O f K), 

A k = k (5.30) 

Since final value of k on the Voronoi boundary is zero. 

2. Evaluation of A6 

Let ^(p.Px) denote the orientation from p to p x and 4 / (p,p 2 ) denote 
the orientation from p to p 2 . Let a be the difference between the orientation 
4 / (p,p 1 ) and the vehicle's orientation 6; j 3 is the difference between the vehicle's 
orientation 6 and x F(p,p 2 ) (Figure 5.4). 
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a = '¥(p,p 1 )-e 
p = e-'¥( P ,p 2 ) 



(5.31) 

(5.32) 



When vehicle is on the Voronoi boundary, 

cc = p (5.33) 

Let the desired orientation be 6 d . At the start point 

e d =nomalize\{ p^^^- -V(p,p,))+ V(p,ft) (5.34) 

Where normalizel is a function which normalizes its argument into a range of 

[ 7t 71 1 

by addition of ±nn if necessary and p 0 is the middle point between p l 
and p 2 . At the other point, 

e d = ^ (5.35) 

Where 6 prtv is the vehicle's previous desired orientation 6 d at the point. Then the 
variable A6 is evaluated as 

a e = e-e d (5.36) 

3. Evaluation of Ad 

Let d, be the distance between p and p,, and d 2 be the distance from p 

to p 2 . 

di =V(*-*i) 2 + Cy-:yi) 2 (5.37) 

dj =^(j:-x 2 ) 2 +(y-y 2 ) 2 (5.38) 

The signed variable Ad is evaluated as follows (Figures 5.5 and 5.6). Note that 
Ad can be signed positive or negative, or equal zero. 
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Ad = d 2 -d, (if x F(p,p 1 )-4'(p,p 2 )>0) (5.39) 

Ad = d l -d 2 (if'P(p, A )-4'( A p 2 )<0) (5.40) 

4. Simulation Results 

The use of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the existing robot 
Yamabico 11. The result is shown in Figures 5.7 and 5.8. Figure 5.7 shows the 
case p, =(0,100), p 2 = (0,-100), the initial configuration of the vehicle is 
<7 = ((-300, 50), 0,0), where there are eight cases of the initial 6 : 0, 45, 90, 135, 
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180, 225, 270, 315 degrees. Figure 5.8 shows the case where the initial 
configuration of the vehicle is # = ((-300, -50), 0,0). 
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200 




C. TWO LINES 

When we navigate the vehicle safely to make the same clearance from two 
lines, its trajectory becomes a line which is a bisector of two directed lines. So it is 
a Voronoi boundary of two lines. Assume a world consists of two directed lines q x 
and q 2 , there are two cases: they are parallel or not parallel (Figures 5.9 and 5.10). 
Interestingly, calculations for A k, A 6 and Ad are identical in each case. 
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Figure 5.9 : Parallel Directed Lines 




Figure 5.10 : Not Parallel Directed Lines 



1. Evaluation of Ak 

When the vehicle's configuration is q = ( p,0,K ), 

A k=k (5.41) 

Since final value of k on Voronoi boundary is zero. 

2. Evaluation of A0 

When the vehicle is on the Voronoi boundary its orientation is the 
average of 0, and 0 2 . Let this desired orientation be 6 d , 
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6 d — normalize — -0,^+0, 



(5.42) 



Where normalize 1 is a function which normalizes its argument into a range of 
% %' 



2’ 2 



by addition of ±nit if necessary. Then the variable A 0 is evaluated as 



A0 = 0-0, 



(5.43) 



3. Evaluation of Ad 

Let d, be the signed distance from p to <?„ and d 2 be the signed 
distance from p to q 2 . 



d, = -(;c-jc 1 )sin0 1 + (y-y 1 )cos0 1 (5.44) 

dj =-{x-x 2 )s\n0 2 + (y-y 2 )cosd 2 (5.45) 

The signed variable Ad is evaluated as 



Ad = *i±<k (5.46) 

The signed distance from p to q l is the distance between p and q r If p 
is on the left of q lt then d, > 0 and if p is on the right of q v then d, < 0 (Figure 
5.1 1). A similar argument holds for dj . 



1 

__ H 


0 

h 


4< ol 

1 


\ <h 

0 



Figure 5.11 : Signed Distance from p to q x 
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4. Simulation Res St 

The result of algorithmic simulation can be found in Figures 5.12, 5.13, 
5.14, and 5.15. 

Figures 5.12 and 5.13 show the case where the lines are parallel. Figure 
5.12 shows the case q l = ((0,100), 0,0), q 2 =((0,-1 00), 0,0) and the initial 
configuration of the vehicle is q = ((0,50), 0,0), where there are eight cases of the 
initial 0 : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.13 shows the case 
where the initial configuration of the vehicle is q = ((0,-50), 0,0). 

Figures 5.14 and 5.15 show the case where the lines are not parallel. 
Figure 5.14 shows the case q l =((0,0), 90,0), q 2 =((0,0),0,0) and the initial 
configuration of the vehicle is q = ((50, 150), 0,0). Figure 5.15 shows the case 
where the initial configuration of the vehicle is q = ((150,50), 0,0). 




Figure 5.12 : Simulation Result of Parallel lines, q = ((0,50), 0,0) 
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Figure 5.15 : Simulation Result of Not Parallel Lines, q = ((150, 50), 0,0) 



D. ONE POINT AND ONE LINE 

When we navigate the vehicle safely to make the same clearance from one 
point and one line, its trajectory becomes a parabola. So it is a Voronoi boundary 
of a point and a line. 

1. Definition of Parabola 

When a world consists of a point p f and a directed line q 0 , its Voronoi 
boundary becomes a parabola. A parabola is defined as the focus p f and the 
directrix q 0 : 

Pf=(x f ,y f ) (5.47) 

<lo=(x o ,y o ,0 o ) (5.48) 

The directrix q 0 has a direction, and hence, parabola has a direction. Let 
1 be the signed distance from p f to q 0 . 
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l=-(x f - * 0 )sin e 0 + (y f - y 0 )cos 6 0 (5.49) 

The signed distance from p f to q 0 is the distance between p f and q 0 . If p f is on 
the left of q 0 , then 1> 0 and if p f is on the right of q 0 , then 1< 0 (Figure 5.16). 




a. Case 1 > 0 

Let 6 X denote the orientation of the normal ray from p f to q 0 . We 
define a polar coordinate system whose pole is p f and whose initial ray is 0, 

(Figure 5.17). 
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e { =d 0 -nl 2 (5.50) 

In this system, p is represented by ( r,</> ), where r is the distance between p f and p 
and <p is the orientation from p f to p taken from the initial ray. In this case, we 
take <t> ( - n < <p < n) counterclockwise from the initial ray. The coordinate of p in 



the global Cartesian system is 



(x,y) = (x f + rcos(0, + <p),y f + rsin(0, + <p )) 

_ r [ lsin(0 o + 0) lcos(0 o + 0) 
[ Xf 1 + COS0 '^ f 1 + COS0 



\ 

J 



(5.51) 



Let 'F(p / ,p) denote the orientation from p f to p. By definition, the angle a 



between ¥( p f ,p ) and the orientation of the tangent at p is defined as 




<P 



2 2 

The orientation of the tangent at p in the polar coordinate system is 



(5.52) 



y = * + « = # + (f-f) = f + f (5.53) 

The orientation 6 of this tangent at p in the global coordinate system is 

e =e 1+ y=(e 0 -f) + (f + f)=f + * 0 (5.54, 

Let s denote the arc length. Then the curvature k atp is 
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de 



1 



r _dd _d<p _ 2 



1 



ds &L ITfFy 2 +r 2 I i 2 r 

d<P y d<p y(l + cos0) 2 (l + cos0) 4 

(l + cos0) 2 _ (1 + cos#) 2 _ 1 



2l>/(l + cos0) 2 +sin 2 0 21^/2 + 2cos0 2-y/21 

1 r 2cos2 fi# = I C osfi] 

2V21V UJJ 1 UJ 



(1 + COS0) 



(5.55) 



b. Case 1 < 0 

In this case (Figure 5.18), the orientation 0, of the initial ray in the 
global coordinate system is 

0, = 0 o + 7r/2 (5.56) 




Figure 5.18 : Parabola ( l< 0) 



We take (f> (-n < <p < n) clockwise from the initial ray. 

= —<p' (5.57) 

Then the coordinate of p in the global Cartesian system is defined by Equation 
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(5.51), the orientation 6 of the tangent at p in the global coordinate system is 
defined by Equation (5.55) and the curvature k at p is defined by Equation 
(5.56). 

2. Evaluation of Ak 

When the vehicle's configuration is q = (p,d,K), assume the intersection 
of the orientation H / (p,p / ) and the parabola is = (p^Q^k^J as shown in 

Figure 5.19. The variable Ak is evaluated as 

A K = K-K para (5.58) 

Note that Ak converges to zero as q approaches to q para . 




Figure 5.19 : Evaluation of Ak 
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3. Evaluation of A0 



Let a be the difference between the vehicle's orientation 6 and the 
orientation 0, of the initial ray. Also let (5 be the difference between the 
orientation '¥(p,p f ) and the vehicle's orientation 6 (Figure 5.20). 




t t 

= 0 O - n / 2 



Figure 5.20 : Evaluation of A6 



Then 





a = ^{p,p f )-d 


(5.59) 




II 

1 


(5.60) 


When vehicle is on the parabola, 


a = P 


(5.61) 


Let this desired orientation be B d , 
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9, = normalized ^IE iEl _ — — - 9 n 1 + 9, 



(5.62) 



Where normalizel is a function which normalizes its argument into a range of 
n n' 



2 2 



by addition of ±nn if necessary. Then the variable A 9 is evaluated as 



A9 = 9-9, 



(5.63) 



4. Evaluation of Ad 

Let d, be the distance between p and p f , and d 2 be the signed distance 
from p to q 0 (Figure 5.21). 

4 =^(x-x f ) 2 +(y-y f ) 2 (5.64) 

d 2 =-(x-x 0 )sin9 0 +(y-y 0 )cosd 0 (5.65) 




Figure 5.21 : Evaluation of Ad 



(5.66) 

(5.67) 



The signed variable Ad is evaluated as 
M = d 2 -d l (if 1>0) 

Ad =d 2 +d, (if 1<0) 
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5. Simulation Result 

The result of algorithmic simulation can be found in Figures 5.22, 5.23, 
5.24 and 5.25. Figures 5.22 and 5.23 show the case where the 1 is positive. 

Figures 5.24 and 5.25 show the case where the 1 is negative. 

Figure 5.22 shows the case of p f =(0,200), q 0 =((0,0), 0,0), the initial 

configuration of the vehicle is q = ((-300, 200), 0,0), where there are eight cases of 
the initial 0 : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.23 shows the 
case where the initial configuration of the vehicle is q = ((-200, 300), 0,0). 

Figure 5.24 shows the case of p f =(0,-200), q 0 =((0,0),0,0), the initial 

configuration of the vehicle is q = ((-300,-200), 0,0), where there are eight cases 
of the initial 0 : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.25 shows the 
case where the initial configuration of the vehicle is q = ((-200,-300), 0,0). 




Figure 5.22 : Simulation Result of 1> 0, q = ((-300, 200), 0,0) 
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Figure 5.23 : Simulation Result of 1> 0, q = ((-200, 300), 0,0) 
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Figure 5.25 : Simulation Result of 1<0, q = ((-200, -300), 0,0) 
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VI. CONCLUSION 



A. RESULTS 

Simulation results shows that when the objects are two points, two lines, or 
one point and one line, we can safely navigate the vehicle to achieve equal 
clearances from these objects. The motion of the vehicle is optimized at each 
point directly from the information of the obstacles near the vehicle. After 
calculating the steering function dK/ds which is the derivative of the curvature of 
the vehicle motion, the vehicle follows the Voronoi boundaries defined by the 
environment. 

Previous work in the motion control of the autonomous vehicle Yamabico 11 
used path tracking using a path specification for lines, circles and parabolas 
which are images of the path. Before the mission began the vehicle was given a 
track to follow; motion planning consisted of calculating the point on the track 
closest to the vehicle and then steering the vehicle to get onto the track. 

If the world navigated by the robot is large, it is complicated and inefficient to 
calculate every Voronoi boundary of this world. It is better to calculate the 
optimal motion of the vehicle at each point directly from the information of the 
world by computing the steering function dK/ds without calculating the track to 
follow. 

Unlike path tracking method, this method can be applied to avoid moving 
objects since we calculate the optimal motion of the vehicle at each point directly 
from the information of the world. Additionally, motion control is simpler and 
faster than in the path tracking method. 
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B. RECOMMENDATIONS 

Based on the results of this thesis, recommended follow on work includes 
leaving point calculation. 

There are two goals for planning autonomous vehicle navigation planning: 
shortest path and safe path. This safe navigation method is for only safe path 
planning. The short path planning is represented by path tracking using a path 
specification for lines, circles and parabolas which are images of the path. When 
we will combine this safe navigation method with short path planning, it will be 
necessary to calculate the leaving point from one path to another. Afterwards, the 
vehicle will continue on its way smoothly. 
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APPENDIX 



This appendix contains the C code for safe navigation which generated the 
results found in this thesis. 



A. POINTPATH.C 

j*1 c************* ****************%**%****%****************** 

Author: Masahide Shirasaka 
Project: Y amabico Robot Control System 
Date: ine 25 1994 

Revised July 12 1994 

File Nar. pointpath.C 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This Program contains functions for safe navigation 

when the obstacles are two points. 

*********************************************** ***********J 

#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 
// =PI 

#define DPI 6.28318530717958647692 
// = 2.0*PI 

#define HPI 1.57079632679489661923 
// = PI/2.0 

#define RAD 57.29577951308232087684 
// = 180.0/PI 

FILE *fp0, *fpl; 

J*1 C** *****************************<- ^ *************** ******** 

struct: POINT 

************************************************** ******** J 

typedef struct { 
double x; 
double y; 

} 

POINT; 

j* * * * * * * * * * * * * * * * * * * % * * * * * * * * * * * * * * * * Jfc * * * * * * * * * * * * * * * * * * * 

struct: CONFIGURATION 
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********************************************************** 

typedef struct { 

POINT point; 
double theta; 
double kappa; 

} 

CONFIGURATION; 



Jif 3fc * * $ * * % $ * $ * $ * * * * $ $ $ $ $ % * * $ jc $ jc $ * jfe $ $ % $ % $ $ $ $ * * % 



Function: normalize() 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI values. 

double normalize(double angle) 

{ 



angle = angle - DPI*(ceil((angle + PI)/DPI) - 1.0); 
retum(angle); 

} 






Function: normalizelO 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI/2.0 values. 

******************************************* ***************j 

double normalizel (double angle) 

{ 



while(angle > PI/2.0) 

{ 

angle = angle - PI; 

} 

while(angle <= -PI/2.0) 
{ 

angle = angle + PI; 

} 

retum(angle); 

} 



j* ******* * * * * * ** * * * * * * * * ** * * * $ $ $ $ $ s|e * * ***** a|c * * * * * * %%%% * * * * * 

FUNCTION: InputPoints() 

Purpose: This procedure Inputs the configurations of two points. 

*************$**$$**$$$$$$***$*$$*$*$$*$ 

void InputPoints(POINT &pl J’OINT &p2) 

{ 

/* Point obstacle pi */ 

printf("Input Coordinates of the pi . \n”); 

printf("X=\n"); 

scanf("%lf ',&pl .x); 
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printf("Y=\n"); 

scanf("%lf',&pl.y); 

/* Point obstacle p2 */ 

printf("Input Coordinates of the p2. \n"); 

printf("X=\n"); 

scanf("%lf , ,&p2.x); 

printf("Y=\n"); 

scanf("%lf',&p2.y); 

} 

FUNCTION: InputlnitConfigO 

Purpose: This procedure Inputs the initial Configration of the vehicle, 

size constant and step size. 

^c^c^c^c^:*****^c^c^c^c*^5lc5lc^^:3|e^:ajc5jc5jc3jc ********************************^ 

void InputInitConfig(CONFIGURATION &q, double &sO,double &DS) 

{ 

/* Config of q */ 

printf( "Input initial Configuration of the vehicle q. \n"); 

printf("X=\n"); 

scanf("%lf",&q.pointx); 

printf("Y=\n"); 

scanf("%lf',&q.poinLy); 

printf("theta= \n"); 

scanf("%lf',&q.theta); 

q.theta=normalize(q.theta/RAD); 

printf("kappa= \n"); 

scanf("%lf',&q.kappa); 

/* Size constant */ 

printf("Input the size constant sChn"); 

printf("sO=Nn"); 

scanf("%lf\&sO); 

/* DS */ 

printf("Input the step size DS.W); 
printf("DS = V’); 
scanf("%lf',&DS); 

} 



I********************************************************** 

FUNCTION: GetInitThetaDesire() 

Purpose: This procedure is for a function which compute the value of 

desired initial theta. 

double GetInitThetaDesire(CONFIGURATION qJ^OINT pi J’OINT p2) 



47 



{ 

POINT pO; 

pO.x = (pl.x + p2.x)/2.0; 

pO.y = (pl.y + p2.y)/2.0; 

retum(normalizel((atan2(pl .y-q.point.y,pl .x-q.point.x) 
+ atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 
- atan2(p0.y-q.point.y,p0.x-q.point.x) ) 

+ atan2(p0.y-q.point.y,p0.x-q.point.x)); 






FUNCTION: GetConstants() 

Purpose: This procedure is for a function which compute the value of 

constants k, a, b, and c. 






**** ****** I 



void GetConstants(double SO, double &a, double &b, double &c) 

{ 

double ConstK; 



ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3 .0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 






FUNCTION: GetSteeringFunc() 

Purpose: This procedure is for a function which compute the value of 

steering function dk/ds. 



void GetSteering(double a,double b, double c, CONFIGURATION q, 

POINT pi .POINT p2, double &thetaDesire, double &u) 

{ 

double deltaKappa, deltaTheta,deltaDist,d 1 ,d2; 



/* Calculate deltaKappa */ 
deltaKappa = q.kappa; 

/* Calculate deltaTheta */ 

thetaDesire = normalizel((atan2(pl.y-q.point.y,pl.x-q.point.x) 
+ atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 
- thetaDesire) + thetaDesire; 
deltaTheta = normalize(q.theta - thetaDesire); 

/* Calculate deltaDist */ 

dl = sqrt((pl.x-q.point.x)*(pl.x-q.point.x) 
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+ (pi .y-q.point.y)*(pl .y-q.point.y)); 
d2 = sqit((p2.x-q.point.x)*(p2.x-q.point.x) 

+ (p2.y-q.point.y)*(p2.y-q.point.y)); 
if (atan2(pl .y-q.point.y,pl .x-q.point.x) 

- atan2(p2.y-q.point.y,p2.x-q.point.x)>0) 
deltaDist = d2 - dl; 

else 

deltaDist = dl - d2; 

/* Calculate Steering fucnction = u */ 
u = -(a*deltaKappa + b*deltaTheta + c*deltaDist); 

} 

J****** **************************************** ************ 

FUNCTION: GetDkappa() 

Purpose: This procedure is for a function which computes the value of 

dKappa. 

********************************************************** J 

double GetDkappa(double u, double ds) 

{ 

retum(u*ds); 

} 

y********************************************************** 

FUNCTION: GetKappa() 

Purpose: This procedure is for a function which computes the value of 

Kappa. 

void GetKappa(double dkappa,CONFIGURATION &q) 

{ 

q.kappa=q.kappa + dkappa; 

} 

FUNCTION: GetDtheta() 

Purpose: This procedure is for a function which computes the value of 

dtheta. 

+ * + % + + + ;f: + + + + + ^ + + + + * + + + + + + + ++ + + + % + / 

double GetDtheta(CONFIGURATION ql, double ds ; 

{ 

retum(ql. kappa * ds); 

} 

^*******:* c***^*******^C3f:***********************3f:*3f:*********** 

FUNCTION: next() 

Purpose: This procedure is for a function which computes the next 

configration of the vehicle. 
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**********************************************************/ 
void next(double ds, double dtheta, double &s, CONFIGURATION &q) 

{ 

CONFIGURATION ql; 

/* CONFIGURATION of ql */ 
ql.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta*dtheta/24.0)*dtheta*ds; 
ql .theta = dtheta; 

s = s + ds* 

/* CONFIGURATION of q */ 

q.point.x = q.point.x + ql.point.x*cos(q.theta) - ql.point.y*sin(q.theta); 
q.point.y = q.point.y + ql.point.x*sin(q.theta) + ql. point. y*cos(q. theta); 
q.theta = q.theta + ql. theta; 

} 

FUNCTION: Openfile() 

Purpose: This procedure opens the output file. 

void Openfile(CONFIGURATION q,double s) 

{ 

fpO = fopen("path.dat","w"); 
fpl = fopen("path","w"); 

fprintf(fpO," s x y theta[deg] kappa "); 
fprintf(fpO," u deltaKappa deltaTheta deltaDistW); 
printf(” s x y theta[deg] kappa\n"); 

fprintf(fp0,"%4.4f %4.4f %4.4f %4.4f %4.4f\n",s, q.point.x, q.point.y, 

q.theta*RAD,q.kappa); 

printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", s, q.point.x, q.point.y, 

q.theta*RAD,q.kappa); 
fprintf(fpl ,"%f %f\n", q.pointx, q.pointy); 

} 

FUNCTION: Printfile() 

Purpose: This procedure prints the result to the file. 

void Printfile(CONFIGURATION q,double s) 

{ 

fprintf(fp0,”%4.4f %4.4f %4.4f %4.4f %4.4f ", 

s, q.point.x, q.pointy, q.theta*RAD,q.kappa); 
printf("%4.4f %4.4f %4.4f%4.4f %4.4f\n", 

s, q.pointx, q.pointy, q.theta*RAD,q.kappa); 
fprintf(fpl,"%f %f\n", q.pointx, q.pointy); /* for gnuplot */ 

} 
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1 $ c X***************************************************;*:**** 

FUNCTION: mainO 

************************:**********************************l 

void main(void) 

{ 

CONFIGURATION q; 

POINT pl,p2; 

double u; /* steering function */ 
double DS,s,sO,a,b,c,thetaDesire; 
double dkappa,dtheta; 

InputPoints(pl ,p2); 

InputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

thetaDesire=GetInitThetaDesire(q,pl ,p2); 

s = 0.0; 

Openfile(q,s); 

do 

{ 

GetSteering(a,b,c,q,pl ,p2,thetaDesire,u); 
dkappa = GetDkappa(u,DS); 

GetKappa(dkappa,q); 

dtheta=GetDtheta(q,DS); 

next(DS,dtbeta,s,q); 

Printfile(q,s); 

} 

while(s<=800.0); 

fclose(fpO); 

fclose(fpl); 

} 



B. LINEPATH.C 

/********************************************************** 

Author: Masahide Shirasaka 

Project: Yamabico Robot Control System 

Date: June 26 1994 

Revised: July 12 1994 

File Name: linepath.C 

Environment: GCC ANSI C compiler for the motorola 68020 processor 
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Description: This Program contains functions for safe navigation 

when the obstacles are two directed lines. 
**********************************************************/ 

#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 
// =PI 

#define DPI 6.28318530717958647692 
// = 2.0*PI 

#define HPI 1.57079632679489661923 
// = PI/2.0 

#define RAD 57.29577951308232087684 
// = 180.0/PI 

FILE *fp0, *fpl; 

struct: POINT 

typedef struct { 
double x; 
double y; 

} 

POINT; 

j*1 c* ************************************************ ******* 

struct: CONFIGURATION 

^c^e^c%^e^c^;^e^:a)c^c^e^c^e%^c^ca)c3|c^c3)c^c^c^ca|c^ca|e^c%^e3)ca)c3)ca(ea)c%^c^e^c^c^e^ca|e^ca)c^e^e%^e^e^c%%a)e^i:^e^e%^ 

typedef struct { 

POINT point; 
double theta; 
double kappa; 

} 

CONFIGURATION; 

/********** ***** *************** ** *************** *********** 
Function: normalize(angle) 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI values. 

********************************************************** j 

double normalize(double angle) 

{ 

angle = angle - DPI*(ceil((angle + PI)/DPI) - 1.0); 
retum(angle); 

} 



52 



Function: normalize 1 (angle) 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI/2.0 values. 

**********************************************************y 

double normalizel (double angle) 

{ 

while( angle > PI/2.0) 

{ 

angle = angle - PI; 

} 

while(angle <= -PI/2.0) 

{ 

angle = angle + PI; 

1 

retum(angle); 

} 



^* ********************************************* 



************ 



FUNCTION: InputLines() 

Purpose: This procedure Inputs the configurations of two Lines. 



void InputLines(CONFIGURATION ^CONFIGURATION &q2) 

{ 

/* Line obstacle ql */ 

printf("Input initial Configuration of ql.Nn"); 

printf("X=\n"); 

scanf("%lf ',&ql .pointx); 

printf("Y=Nn"); 

scanf("%lf',&ql .point.y); 

printf("theta= W); 

scanf("%lf',&ql .theta); 

ql .theta=normalize(ql .theta/RAD); 

ql.kappa=0.0; 



/* Line obstacle q2 */ 

piintf("Input initial Configuration of q2.\n"); 

printf("X=Nn"); 

scanf("%lf',&q2.point.x); 

printf("Y=Nn"); 

scanf("%lf',&q2.point.y); 

printf("theta= Nn"); 

scanf(" %lf ' ,&q2. theta); 

q2.theta=normalize(q2.theta/RAD); 

q2.kappa=0.0; 

} 
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/sic*************************************************;******** 

FUNCTION: InputlnitConfigQ 

Purpose: This procedure Inputs the initial Configration of the vehicle, 

size constant and step size. 

void InputInitConfig(CONFIGURATION &q,double &s0, double &DS) 

{ 

/* Config of q */ 

printf("Input initial Configuration of the vehicle q. \n"); 

printf("X=\n"); 

scanf("%lf',&q.pointx); 

printf("Y=\n"); 

scanf("%lf',&q.pointy); 

printf("theta= \n"); 

scanf("%lf',&q.theta); 

q.theta=normalize(q.theta/RAD); 

printf("kappa= \n"); 

scanf(" %lf' ,&q.kappa); 

/* Size constant */ 

printf("Input the size constant sChn"); 
printf("sO =\n"); 
scanf("%lf\&sO); 

/* DS */ 

printf("Input the step size DS.Nn"); 
printf("DS =W); 
scanf("%lf',&DS); 

} 

/********************************************************** 
FUNCTION: GetConstants() 

Purpose: This procedure is for a function which computes the value of 

constants k, a, b, and c. 

void GetConstants(double S0,double &a, double &b,double &c) 

{ 

double ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3 .0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 

FUNCTION: GetSteeringFuncQ 
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Purpose: This procedure is for a function which computes the value of 

steering function dk/ds. 

double GetSteering(double a,double b, double c, CONFIGURATION q, 
CONFIGURATION ql .CONFIGURATION q2) 



{ 



double deltaKappa,deltaTheta,deltaDist,thetaDesire,dl ,d2; 



/* Calculate DeltaKappa */ 
deltaKappa = q.kappa; 



/* Calculate DeltaTheta */ 

thetaDesire = normalizel((ql.theta+q2.theta)/2.0 - ql .theta) + ql. theta; 
deltaTheta = normalize(q.theta - thetaDesire); 

/* Calculate DeltaDist */ 

dl = -(q.point.x - ql.point.x)*sin(ql. theta) 

+ (q.pointy - ql.point.y)*cos(ql. theta); 
d2 = -(q.point.x - q2.point.x)*sin(q2.theta) 

+ (q.point.y - q2.point.y)*cos(q2.theta); 
deltaDist = (dl + d2)/2.0; 



/* Calculate Steering fucnction = u */ 
retum(-(a*deltaKappa + b*deltaTheta + c*deltaDist)); 

} 



FUNCTION: GetDkappa() 

Purpose: This procedure is for a function which computes the value of 

dKappa. 

double GetDkappa(double u, double ds) 

{ 

retum(u*ds); 

} 



/* 






FUNCTION: GetKappa() 

Purpose: This procedure is for a function which computes the value of 

Kappa. 






void GetKappa(double dkappa, CONFIGURATION &q) 

{ 

q.kappa=q. kappa + dkappa; 

} 
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FUNCTION: GetDtheta() 

Purpose: This procedure is for a function which computes the value of 

dtheta. 

double GetDtheta(CONFIGURATION ql, double ds) 

{ 

retum(ql. kappa * ds); 

} 

FUNCTION: next() 

Purpose: This procedure is for a function which computes the next 

configration of the vehicle. 

void next(double ds, double dtheta, double &s, CONFIGURATION &q) 

{ 

CONFIGURATION ql; 

/* CONFIGURATION of ql */ 
ql.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta *dtheta/24.0)*dtheta*ds; 
ql. theta = dtheta; 

s = s + ds; 

/* CONFIGURATION of q */ 

q.point.x = q.point.x + ql. point. x*cos(q.theta) - ql. pointy *sin(q.theta); 
q.pointy = q.pointy + ql.point.x*sin(q.theta) + ql .point.y*cos(q.theta); 
q.theta = q.theta + ql .theta; 

} 

/St********************************************************* 

FUNCTION: Openfile() 

Purpose: This procedure opens the output file. 

**********************************************************/ 
void Openfile(CONFIGURATION q,double s) 

{ 

fpO = fopen("path.dat”,"w"); 
fpl = fopen("path","w"); 

fprintf(fpO," s x y theta[deg] kappa "); 
fprintf(fpO," u deltaKappa deltaTheta deltaDistNn"); 
printf(" s x y thetafdeg] kappaW'); 

fprintf(fp0,"%4.4f %4.4f %4.4f %4.4f %4.4Ni",s, q.point.x, q.pointy, 

q.theta*RAD,q.kappa); 

printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", s, q.pointx, q.pointy, 

q.theta*RAD,q.kappa); 
fprintf(fpl,"%f %fvn", q.pointx, q.pointy); 
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/**** C + ^CSIC^^************************************************* 

FUNCTION: Printfile() 

Purpose: This procedure prints the result to the file. 

void Printfile(CONFIGURATION q,double s) 

{ 

fprintf(fp0,"%4.4f %4.4f %4.4f %4.4f %4.4f ", 

s, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", 

s, q.pointx, q.point.y, q.theta*RAD,q.kappa); 
fprintf(fpl,"%f %f\n", q.pointx, q.point.y); /* for gnuplot */ 

} 

^sfcafrsfralcafcafrafrafrafrafratcsfcafr^cafc ******************************************* 

FUNCTION: mainO 

**********************************************************/ 
void main(void) 

{ 

CONFIGURATION q,ql,q2; 
double u; /* steering function */ 
double DS,s,sO,a,b,c; 
double dkappa.dtheta; 

InputLines(ql ,q2); 

InputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

s = 0.0; 

Openfile(q,s); 

do 

{ 

u = GetSteering(a,b,c,q,ql,q2); 
dkappa = GetDkappa(u,DS); 

GetKappa(dkappa,q); 

dtheta=GetDtheta(q,DS); 

next(DS,dtheta,s,q); 

Printfile(q,s); 

} 

while(s<=800.0); 

fclose(fpO); 

fclose(fpl); 

} 
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C. PARAPATH.C 



Author: Masahide Shirasaka 

Project: Yamabico Robot Control System 

Date: May 15 1994 

Revised: June 17 1994 

File Name: parapath.C 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This Program contains functions for safe navigation 

when the obstacles are one point and one directed line. 
************************************************ **********y 

#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 
// =PI 

#define DPI 6.28318530717958647692 
// = 2.0*PI 

#define HPI 1.57079632679489661923 
// = PI/2.0 

#define RAD 57.29577951308232087684 
// = 180.0/PI 

FILE *fp0, *fpl, *fp2, *fp3; 



J** ******************************************* ************* 

struct: POINT 

typedef struct { 
double x; 
double y; 

} 

POINT; 

J** ******************************************************** 

struct: CONFIGURATION 

typedef struct { 

POINT point; 
double theta; 
double kappa; 

} 

CONFIGURATION; 
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f* afc afc afc afc afc sfc afc afc a|c sfc afc sfc sfc afe afc afc sfc afc a|c afe 3fc afc ^fc afc sfc a|c afe sfc afc afc a|c sfc afc afc afc a|c afc afc sfe afc a|c afc a|c afe afc sfc afc 3fe a|c afc afc sfc 

Function: normalize() 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI values. 

a|c a|e afe sfe afc afc sfc a^c afc sfc s|c sfc afc s|c sfc sfc ^|c ^|c afc s|c 3|c a|c 3|c 3^c afc afc sfc sfc afc afc afe afc s|c s|c sfc ^fc ^fc afc a|c afc 3|c 3|c afe afc 3fc sfc d|e sfc afc afc afc a|c ^ afc sfe sfe ^ 

double normalize(double angle) 

{ 

angle = angle - DPI*(ceil((angle + PI)/DPI) - 1 .0); 
retum(angle); 

} 

Function: normalizel() 

Purpose: This procedure is for a function which normalizes an angle 

to within + or - PI/2.0 values. 

* *********************************************** **********/ 
double normalize 1 (double angle) 

I 

while(angle > PI/2.0) 

{ 

angle = angle - PI; 

} 

while(angle <= -PI/2.0) 

{ 

angle = angle + PI; 

} 

retum(angle); 

} 

ya|c^c^c^c^c^e^c^c^c^:^c)(c^c^c^c^c^c^c^c^c^c^e^c^c^c^c^c^c^c^c^c^c^c^c^e^c^c^c^c^c^c^c^c^c)|c)lc^c^c^c))c^c^c^c^c^c^c^c^c 

FUNCTION: InputParabola() 

Purpose: This procedure Inputs the Configrations of one point and 

one directed line. 

void InputParabola(CONFIGURATION &qO,POINT &p) 

{ 

/* Config of qO */ 

printf("Input initial Configuration of the qO (directrix). Nn"); 

printf("X=W); 

scanf("%lf',&q0.point.x); 

printf("Y=\n"); 

scanf("%lf’,&q0.point.y); 

printf("theta= Nn"); 

scanf("%lf’,&q0.theta); 

q0.theta=normalize(q0.theta/RAD); 

q0.kappa=0.0; 
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/* Point obstacle */ 

printf("Input Coordinates of the pf (focus). \n"); 

printf("X=\n”); 

scanf("%lf',&p.x); 

printf("Y=\n"); 

scanf("%lf',&p.y); 

} 






FUNCTION: InputInitConfig() 

Purpose: This procedure Inputs the initial Configration of the vehicle, 

size constant and step size. 









void InputInitConfig(CONFIGURATION &q,double &s0, double &DS) 

{ 

/* Config of q */ 

printf("Input initial Configuration of the vehicle q. \n"); 

printf("X=\n"); 

scanf("%lf’,&q.pointx); 

printf("Y=\n"); 

scanf("%lf’,&q.pointy); 

printf("theta= \n"); 

scanf("%lf’,&q.theta); 

q.theta=normalize(q .theta/R AD); 

printf("kappa= Nn"); 

scanf(" %lf ’,&q.kappa); 



/* Size constant*/ 

printf("Input the size constant sCNn"); 
printf("sO =Nn"); 
scanf("%lf’,&sO); 



/* DS */ 

printf("Input the step size DS.Nn"); 
printffDS = V); 
scanf("%lf’,&DS); 

} 



' FUNCTION: GetSize() 

Purpose: This procedure is for a function which computes the value of 

size of the parabola. 

double GetSize(CONFIGURATION qO^OINTp) 

{ 

retum(-(p.x-q0.point.x)*sin(q0.theta) + (p.y-q0.point.y)*cos(q0.theta)); 

} 
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FUNCTION: GetConstants() 

Purpose: This procedure is for a function which computes the value of 

constants k, a, b, and c. 

**********************************************************^ 
void GetConstants(double SO, double &a,double &b, double &c) 

{ 

double ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3.0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 

^* ********************************************************* 
FUNCTION: GetSteeringFunc() 

Purpose: This procedure is for a function which computes the value of 

steering function dk/ds. 

************************************************ ********* *y 

double GetSteering(double a,double b,double ^CONFIGURATION q, 
CONFIGURATION qOJPOINT p, double size) 

{ 

double kappaPara,phi,deltaKappa,thetaN,thetaDesire, 
deltaTheta,deltaDist,dl ,d2; 

/* Calculate DeltaKappa */ 
if ( size >= 0.0 ) 
phi = 

normalize(atan2(q.point.y-p.y, q.point.x-p.x) - (q0.theta-PI/2.0)); 

else 

phi = normalize(-atan2(q.point.y-p.y, q.point.x-p.x) + 

(q0.theta+PI/2.0)); 

kappaPara = cos(phi/2.0)*cos(phi/2.0)*cos(phi/2.0)/size; 
deltaKappa = q.kappa - kappaPara; 

/* Calculate DeltaTheta */ 

thetaN=((size >= 0.0) ? (qO.theta - PI/2.0):(q0.theta + PI/2.0)); 
thetaDesire = 

normalizel((atan2(p.y-q.point.y, p.x-q.point.x) + thetaN)/2.0 - qO.theta) 

+ qO.theta; 

deltaTheta = normalize(q.theta - thetaDesire); 

/* Calculate DeltaDist */ 

dl = sqrt((p.x-q.point.x)*(p.x-q.point.x) + (p.y-q.point.y)*(p.y-q.point.y)); 
d2 = -(q.point.x-q0.point.x)*sin(q0.theta) 

+ (q.point.y-q0.point.y)*cos(q0.theta); 
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if (size >= 0.0) 

deltaDist = d2 - dl; 

else 

deltaDist = d2 + dl; 

/* Calculate Steering fucnction = u */ 
retum(-(a*deltaKappa + b*deltaTheta + c*deltaDist)); 

} 

FUNCTION: GetDkappa() 

Purpose: This procedure is for a function which computes the value of 

dKappa. 

**********************************************************/ 
double GetDkappa(double u, double ds) 

{ 

retum(u*ds); 

} 

1 FUNCTION: GetKappa() 

Purpose: This procedure is for a function which computes the value of 

Kappa. 

**************************************** **************** **j 

void GetKappa(double dkappa, CONFIGURATION &q) 

{ 

q.kappa=q.kappa + dkappa; 

} 

^ 3#c a|c 3fc a|c afc afe a|c a|c s#c afc a|c a|c afc 3|c 3#c afc a|c a|c a|c a|e a|e a|e a|c a|c a|c a#c s#c afc a|c a|c a|c a|c a|c a|c afc a|e afc a#c a#c sfc 3fc a|c afc 5#c a#c a#c 3#c a|e a|c 3#c a#c a#c 3#c a#c 

FUNCTION: GetDtheta() 

Purpose: This procedure is for a function which computes the value of 

dtheta. 

3#e a#c afe afc a#c afe afc a|e s|c a|c afc a(c afc afc a|c a|c afc a|c afc a|c a|c afe afc afc afc a|c a|c afc afc afc afc a|c a|c afc afc afe afc a|c a|c a#c afe s#c 5#c a(c s#c afc afc affc a|c a|c 3fc sfc afc ^ 

double GetDtheta(CONFIGURATION ql, double ds) 

{ 

retum(ql. kappa * ds); 

} 

^)* ********************************************************* 
FUNCTION: next() 

Purpose: This procedure is for a function which computes the next 

configration of the vehicle. 

****************************************************** ****^ 
void next(double ds, double dtheta,double &s,C0NHGURAT10N &q) 

{ 

CONFIGURATION ql; 
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/* CONFIGURATION of ql */ 
ql.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta*dtheta/24.0)*dtheta*ds; 
ql .theta = dtheta; 



s = s + ds* 

/* CONFIGURATION of q */ 

q.point.x = q.point.x + ql.point.x*cos(q.theta) - ql.point.y*sin(q.theta); 
q.point.y = q.point.y + ql.point.x*sin(q.theta) + ql.point.y*cos(q.theta); 
q. theta = q.theta + ql. theta; 

} 



/***** 



***************************************************** 



FUNCTION: Openfile() 

Purpose: This procedure opens the output file. 

******************************** ************************** 



void Openfile(CONFlGURATION q,double s) 

{ 

fpO = fopen("path.dat", "w"); 
fpl = fopenfpath’V’w"); 

fprintf(fpO," s x y theta[deg] kappa "); 
fprintf(fpO," u deltaKappa deltaTheta deltaDistW); 
printf(" s x y theta[deg] kappaW); 

fprintf(fp0,"%4.4f %4.4f %4.4f %4.4f %4.4fW',s, q.pointx, q.point.y, 

q.theta*RAD,q.kappa); 

printf("%4.4f %4.4f %4.4f %4.4f %4.4Ni", s, q.point.x, q.point.y, 

q.theta*RAD,q.kappa); 
fprintf(fpl ,"%f %fV', q.pointx, q.pointy); 

} 



/********************************************************** 
FUNCTION: PrintfileO 

Purpose: This procedure prints the result to the file. 

******************************************************** **yf 

void Printfile(CONFIGURATION q,double s) 

{ 

fprintf(fp0,"%4.4f %4.4f %4.4f %4.4f %4.4f ", 

s, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
printf("%4.4f %4.4f %4.4f %4.4f %4.4Ni", 

s, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
fprintf(fpl,"%f %f\n", q.point.x, q.point.y); /* for gnuplot */ 

} 

r* ********************************************************* 

FUNCTION: mainO 

^*********************************************************/ 
void main(void) 
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{ 

CONFIGURATION qO,q; 

POINT p; 

double u; /* steering function */ 
double DS,s,sO,a,b,c,size; 
double dkappa,dtheta; 

InputParabola(qO,p); 

size=GetSize(qO,p); 

InputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

s = 0.0; 

Openfile(q,s); 

do 

{ 

u = GetSteering(a,b,c,q,qO,p,size); 
dkappa = GetDkappa(u,DS); 
GetKappa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 

Printfile(q,s); 

} 

while(s<=2000.0); 

fclose(fpO); 

fclose(fpl); 

} 
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